#include "module/edit_calibration_parameters.h"

#include <iostream>
#include <fstream>
#include <yaml-cpp/yaml.h>
#include <ros/package.h>
#include <QtDebug>

namespace camera_calibration {
EditCalibrationParameters::EditCalibrationParameters() {}

void EditCalibrationParameters::ReadGridDefaultParameters(
    int &_grid_x, int &_grid_y, double &_grid_width) {

  std::string package_path = ros::package::getPath("camera_calibration_studio");

  std::string config_path = package_path + "/config/default_parameters.yaml";

  YAML::Node config = YAML::LoadFile(config_path);
  YAML::Node grid = config["grid"];
  _grid_x = grid["grid_x"].as<int>();
  _grid_y = grid["grid_y"].as<int>();
  _grid_width = grid["grid_width"].as<double>();
}

void EditCalibrationParameters::EditOstFile(const QString _store_lib) {
  std::string package_path = ros::package::getPath("camera_calibration_studio");

  std::string edit_config_path = package_path + "/config/edit_ost.yaml";
  std::string store_path = _store_lib.toStdString();

  YAML::Node ost_config = YAML::LoadFile(store_path);
  YAML::Node edit_ost_config = YAML::LoadFile(edit_config_path);

  YAML::Node camera_matrix =ost_config["camera_matrix"];
  YAML::Node camera_data = camera_matrix["data"];

  YAML::Node distortion_coefficients = ost_config["distortion_coefficients"];
  YAML::Node coefficients_data = distortion_coefficients["data"];

  edit_ost_config["camera_matrix"]["data"] = camera_data;
  edit_ost_config["distortion_coefficients"]["data"] = coefficients_data;

  std::ofstream fout(store_path);
  if (!fout.is_open()) {
    throw std::runtime_error("无法打开文件进行写入: " + store_path);
  }

  fout << edit_ost_config;
  fout.close();
}

} //namespace camera_calibration
